#include "StdAfx.h"
#include "ArmDevice.h"
#include "RobotInstance.h"

namespace RobotPlant { namespace RobotProxy{

	ArmDevice::ArmDevice(RobotInstance* pParent,joint::spawn_type e):DeviceBase(pParent,e)
	{
		_deviceType = DeviceTypeEnum::Arm;
		_deviceIndex = 1;
		chord(GetCurrentJointPose, &ArmDevice::Handler_GetCurrentJointPose);
		chord(GetCurrentJointVel, &ArmDevice::Handler_GetCurrentJointVel);
		chord(MoveJoint, &ArmDevice::Handler_MoveJoint);
		chord(Stop, &ArmDevice::Handler_Stop);
	}

	ArmDevice::~ArmDevice()
	{
	}

	std::vector<double> ArmDevice::Handler_GetCurrentJointPose(void_t)
	{
		boost::mutex::scoped_lock lock(this->_pParent->_mutex);

		std::vector<double> pose = std::vector<double>();

		RobDeviceInterface robDeviceInterface;
		string addr = _pParent->_soapServer;
		robDeviceInterface.endpoint = addr.data();
		DoubleArray temp;
		temp.__ptr = new double[7];
		temp.__size = 7;
		temp.__offset = 0;
		if (robDeviceInterface.ns1__Arm_GetCurrentJointPose(this->_deviceIndex,&temp) != SOAP_OK)
		{
			pose.clear();
		}
		else
		{
			for(int i=0; i<7; i++)
				pose.push_back(temp.__ptr[i]);
		}
		delete[] temp.__ptr;

		return pose;
	}

	std::vector<double> ArmDevice::Handler_GetCurrentJointVel(void_t)
	{
		boost::mutex::scoped_lock lock(this->_pParent->_mutex);

		std::vector<double> vel = std::vector<double>();

		RobDeviceInterface robDeviceInterface;
		string addr = _pParent->_soapServer;
		robDeviceInterface.endpoint = addr.data();
		DoubleArray temp;
		temp.__ptr = new double[7];
		temp.__size = 7;
		temp.__offset = 0;
		if (robDeviceInterface.ns1__Arm_GetCurrentJointVel(this->_deviceIndex,&temp) != SOAP_OK)
		{
			vel.clear();
		}
		else
		{
			for(int i=0; i<7; i++)
				vel.push_back(temp.__ptr[i]);
		}
		delete[] temp.__ptr;

		return vel;
	}

	void ArmDevice::Handler_MoveJoint(MoveJointArg arg)
	{
		boost::mutex::scoped_lock lock(this->_pParent->_mutex);

		RobDeviceInterface robDeviceInterface;
		string addr = _pParent->_soapServer;
		robDeviceInterface.endpoint = addr.data();
		DoubleArray toJointValues;
		toJointValues.__ptr = new double[7];
		toJointValues.__size = 7;
		toJointValues.__offset = 0;
		ns1__EmptyStruct ret;
		if (robDeviceInterface.ns1__Arm_MoveJoint(this->_deviceIndex, toJointValues, arg.speed, arg.flyByZone, &ret) != SOAP_OK)
		{
			//value = -1;
		}
	}

	void ArmDevice::Handler_Stop(bool followPath)
	{
		boost::mutex::scoped_lock lock(this->_pParent->_mutex);

		RobDeviceInterface robDeviceInterface;
		string addr = _pParent->_soapServer;
		robDeviceInterface.endpoint = addr.data();
		xsd__boolean arg = false_;
		if(followPath == true)
			arg = true_;
		ns1__EmptyStruct ret;
		if (robDeviceInterface.ns1__Arm_Stop(this->_deviceIndex, arg, &ret) != SOAP_OK)
		{
			//value = -1;
		}
	}
}}